#include "extension_controller.h"
#include <iostream>
using namespace std;
using namespace adevs;

const double ExtensionController::DEFAULT_PHASE_LENGTH = 20.0;
const double ExtensionController::DEFAULT_PHASE_MAX_LENGTH = 60.0;
const double ExtensionController::DEFAULT_EXTENSION_STEP = 5.0;

ExtensionController::ExtensionController():
	Controller(), // Initialize the parent Atomic model
	// Initialize status variables
	remaining_time(0.0),
	current_side_green(-1),
	phase_time(0.0),
	// Initialize setting variables
	phase_max_length(DEFAULT_PHASE_MAX_LENGTH),
	extension_step(DEFAULT_EXTENSION_STEP)
{
	// Initialize setting variables
	for (int i = 0; i < 4; i++)
		phase_length[i] = DEFAULT_PHASE_LENGTH;
}

void ExtensionController::delta_ext(double e, const Bag<IO_Type>& xb)
{
	remaining_time -= e;
	phase_time += e;
	// Phase length extension
	if (phase_time + extension_step > current_phase_length)
	{
		double new_phase_length = phase_time + extension_step;
		if (new_phase_length > phase_max_length)
			new_phase_length = phase_max_length;
		remaining_time += (new_phase_length - current_phase_length);
		current_phase_length = new_phase_length;
	}

	Bag<IO_Type>::const_iterator i = xb.begin();
	for (; i != xb.end(); i++)
	{

	}
}

void ExtensionController::delta_int()
{
	phase_time += remaining_time;
	//cout << "Pahase " << current_side_green << " lasted " << phase_time << endl;
	// Switch to next phase
	current_side_green = next_side_green();
	remaining_time = phase_length[current_side_green];
	current_phase_length = phase_length[current_side_green];
	phase_time = 0.0;
}

void ExtensionController::delta_conf(const Bag<IO_Type>& xb)
{
	delta_int();
	delta_ext(0.0,xb);
}

void ExtensionController::output_func(Bag<IO_Type>& yb)
{
	if (current_side_green != -1)
	{
			IO_Type turn_off(out_semaphor_cmd(current_side_green), (io_type*)new SemaphorCommand(RED));
			yb.insert(turn_off);
	}
	IO_Type turn_on(out_semaphor_cmd(next_side_green()), (io_type*)new SemaphorCommand(GREEN));
	yb.insert(turn_on);
}

double ExtensionController::ta()
{
	if (current_side_green == -1) return 0;

	return remaining_time;
}

void ExtensionController::gc_output(Bag<IO_Type>& g)
{
	Bag<IO_Type>::iterator i;
	for (i = g.begin(); i != g.end(); i++)
	{
		delete (*i).value;
	}
}

ExtensionController::~ExtensionController()
{
}

